/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#ifndef B2_MATH_H
#define B2_MATH_H

#include "b2Settings.h"

#include <math.h>
#include <float.h>
#include <stddef.h>

#ifdef TARGET_FLOAT32_IS_FIXED

inline Fixed b2Min(const Fixed& a, const Fixed& b)
{
	return a < b ? a : b;
}

inline Fixed b2Max(const Fixed& a, const Fixed& b)
{
	return a > b ? a : b;
}

inline Fixed b2Clamp(Fixed a, Fixed low, Fixed high)
{
	return b2Max(low, b2Min(a, high));
}

inline bool b2IsValid(Fixed x)
{
	B2_NOT_USED(x);
	return true;
}

#define	b2Sqrt(x)	sqrt(x)
#define	b2Atan2(y, x)	atan2(y, x)

#else

///This function is used to ensure that a floating point number is 
///not a NaN or infinity.
inline bool b2IsValid(float32 x)
{
#ifdef _MSC_VER
	return _finite(x) != 0;
#else 

#ifdef TARGET_OS_IPHONE
	return isfinite(x);
#else
	return finite(x) != 0;
#endif

#endif
}

///This is approximate yet fast inverse square-root.
inline float32 b2InvSqrt(float32 x)
{
	union 
	{
		float32 x;
		int32 i;
	}convert;
	convert.x = x;
	float xhalf = 0.5f * x;
	convert.i = 0x5f3759df - (convert.i >> 1);
	x = convert.x;
	x = x * (1.5f - xhalf * x * x);

	return x;
}

#define b2Sqrt(x)	sqrtf(x)
#define b2Atan2(y, x)	atan2f(y, x)

#endif

inline float32 b2Abs(float32 a)
{
	return a>0.0f ? a : -a;
}

/// A 2D column vector.
struct b2Vec2
{
	///Default constructor does nothing (for performance).
	b2Vec2()	{}

	///Construct using coordinates.
	b2Vec2(float32 x, float32 y) : x(x), y(y)	{}

	///Set this vector to all zeros.
	void SetZero()	{ x = 0.0f; y = 0.0f; }

	///Set this vector to some specified coordinates.
	void Set(float32 _x, float32 _y) { x = _x; y = _y; }

	///Negate this vector.
	b2Vec2 operator -()	{ b2Vec2 v; v.Set(-x, -y); return v; }

	///Read from an indexed element.
	float32& operator ()(int i) { return (&x)[i]; }

	///Add a vector to this Vector.
	void operator +=(const b2Vec2& v)	{ x += v.x; y += v.y; }

	///Subtract a vector from this vector.
	void operator -=(const b2Vec2& v)	{ x -= v.x; y -= v.y; }

	///Multiply this vector by a scalar.
	void operator *=(const float32 a)	{ x *= a; y *= a; }

	///Get the length of this vector.
	float32 Length() const	
	{ 
#ifdef TARGET_FLOAT32_IS_FIXED
		float est = b2Abs(x) + b2Abs(y);
		if(est == 0.0f) {
			return 0.0;
		} else if(est < 0.1) {
			return (1.0/256.0) * b2Vec2(x<<8, y<<8).Length();
		} else if(est < 180.0f) {
			return b2Sqrt(x * x + y * y);
		} else {
			return 256.0 * (b2Vec2(x>>8, y>>8).Length());
		}
#else
		return b2Sqrt(x * x + y * y);
#endif 
	}

	///Get the length squared, for performance, use this instead of 
	///b2Vec2::Length.
	float32 LengthSquared() const
	{
		return x*x + y*y; 
	}

	///Convert this vector into a unit vector and return the length.
#ifdef TARGET_FLOAT32_IS_FIXED
	float32 Normalize()
	{
		float32 length = Length();
		if (length < B2_FLT_EPSILON)
		{
			return 0.0f;
		} 
#ifdef NORMALIZE_BY_INVERT_MULTIPLY
		if (length < (1.0/16.0)) {
			x = x << 4;
			y = y << 4;
			return (1.0/16.0)*Normalize();
		} else if(length > 16.0) {
			x = x >> 4;
			y = y >> 4;
			return 16.0*Normalize();
		}
		float32 invLength = 1.0f / length;
		x *= invLength;
		y *= invLength;
#else
		x /= length;
		y /= length;
#endif
		return length;
	}
#else

	float32 Normalize()
	{
		float32 length = Length();
		if(length < B2_FLT_EPSILON)
		{
			return 0.0; 
		}
		float invLength = 1.0f / length;
		x *= invLength;
		y *= invLength;

		return length;
	}
#endif

	///Does this vector contain finite coordinates.
	bool IsValid() const	{ return (b2IsValid(x) && b2IsValid(y)); }

	float32 x, y;
};

///A 2D column vector with 3 element.
struct b2Vec3
{
	///Default constructor  does nothing for performance.
	b2Vec3()	{}

	///Construct using coordinates.
	b2Vec3(float32 _x, float32 _y, float32 _z) : x(_x), y(_y), z(_z)	{}

	///Set this vector to all zero.
	void SetZero()	{ x = 0.0f; y = 0.0f; z = 0.0f; }

	///Set this vector using some specified coordinates.
	void Set(float32 _x, float32 _y, float32 _z)	{ x = _x; y = _y; z = _z; }

	///Add a vector to this vector.
	void operator +=(const b2Vec3& v)	{ x += v.x; y += v.y; z += v.z; }
	
	///Subtract this vector from a vector.
	void operator -=(const b2Vec3& v)	{ x -= v.x; y -= v.y; z -= v.z; }

	//Multiply this vector by a scalar.
	void operator *=(float32 a)		{ x *= a; y *= a; z *= a; }

	float32 x, y, z;
};

///A 2-by-2 matrix, Stored in column-major order.
struct b2Mat22
{
	///The default constructor does nothing (for performance).
	b2Mat22()	{}

	///Construct this matrix using columns.
	b2Mat22(const b2Vec2& c1, const b2Vec2& c2)	{ col1 = c1, col2 = c2; }

	///Construct this matrix using scarlar.
	b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)	{ col1.x = a11; col2.x = a12; col1.y = a21; col2.y = a22; }

	///Construct this vector using an angle.this matrix becomes an orthonormal rotation matrix.
	explicit b2Mat22(float32 angle)
	{
		float32 c = cosf(angle), s = sinf(angle);
		col1.x = c; col2.x = -s;
		col1.y = s; col2.y = c;
	}

	///Initialize this Matrix using columns.
	void Set(const b2Vec2& c1, const b2Vec2& c2)	{	col1 = c1; col2 = c2; }

	///Initialize this vector using an angle, this matrix becomes a orthnormal rotation matrix.
	void Set(float32 angle)	
	{
		float32 c = cosf(angle) ,s = sinf(angle);
		col1.x = c; col2.x = -s;
		col2.x = s; col2.y = c;
	}

	///Initialize this matrix to the identity matrix.
	void SetIdentity()
	{
		col1.x = 1.0f; col2.x = 0.0f;
		col1.y = 0.0f; col2.y = 1.0f;
	}

	///Set this matrix to all zero.
	void SetZero()
	{
		col1.x = 0.0f; col2.x = 0.0f;
		col1.y = 0.0f; col2.y = 0.0f;
	}

	///Extract the angle from this matrix(assume this matrix to be a rotation matrix.)
	float32 GetAngle()
	{
		return b2Atan2(col1.y, col1.x);
	}

#ifdef TARGET_FLOAT32_IS_FIXED

	/// Compute the inverse of this matrix, such that inv(A) * A = identity.
	b2Mat22 GetInverse() const
	{
		float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
		float32 det = a * d - b * c;
		b2Mat22 B;
		int n = 0;

		if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
		{
			n = 3;
			a = a<<n; b = b<<n; 
			c = c<<n; d = d<<n;
			det = a * d - b * c;
			b2Assert(det != 0.0f);
			det = float32(1) / det;
			B.col1.x = ( det * d) << n;	B.col2.x = (-det * b) << n;
			B.col1.y = (-det * c) << n;	B.col2.y = ( det * a) << n;
		} 
		else
		{
			n = (b2Abs(det) >= 16.0)? 4 : 0;
			b2Assert(det != 0.0f);
			det = float32(1<<n) / det;
			B.col1.x = ( det * d) >> n;	B.col2.x = (-det * b) >> n;
			B.col1.y = (-det * c) >> n;	B.col2.y = ( det * a) >> n;
		}

		return B;
	}

	// Solve A * x = b
	b2Vec2 Solve(const b2Vec2& b) const
	{
		float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
		float32 det = a11 * a22 - a12 * a21;
		int n = 0;
		b2Vec2 x;


		if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
		{
			n = 3;
			a11 = col1.x<<n; a12 = col2.x<<n;
			a21 = col1.y<<n; a22 = col2.y<<n;
			det = a11 * a22 - a12 * a21;
			b2Assert(det != 0.0f);
			det = float32(1) / det;
			x.x = (det * (a22 * b.x - a12 * b.y)) << n;
			x.y = (det * (a11 * b.y - a21 * b.x)) << n;
		} 
		else 
		{
			n = (b2Abs(det) >= 16.0) ? 4 : 0;
			b2Assert(det != 0.0f);
			det = float32(1<<n) / det;
			x.x = (det * (a22 * b.x - a12 * b.y)) >> n;
			x.y = (det * (a11 * b.y - a21 * b.x)) >> n;
		}

		return x;
	}

#else

	///Compute the inverse of this matrix, such that inv(A) * a = Identity.
	b2Mat22 GetInverse() const
	{
		float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y; 
		b2Mat22 B;
		float32 det = a*d - b*c;
		b2Assert(0.0f != det);
		det = 1.0f / det;
		B.col1.x = d * det; B.col2.x = -c * det;
		B.col1.y = -b * det; B.col2.y = a * det;
		return B;
	}

	///Solve A * x = b, where b is a column vector.
	b2Vec2 Solve(const b2Vec2& b)
	{
		float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
		float32 det = a11*a22 - a21*a12;
		b2Assert(0.0f != det);
		det = 1.0f / det;

		b2Vec2 x;
		x.x = (b.x*a22 - b.y*a12) * det;
		x.y = (a11*b.y - a21*b.x) * det;
		return x;
	}
#endif

	b2Vec2 col1, col2;
};

///A 3-by-3 matrix, stored in column-major order.
struct b2Mat33
{
	///Default constructor does nothing.(for performance)
	b2Mat33()	{}

	///Construct this matrix use columns.
	b2Mat33(const b2Vec3& c1,const b2Vec3& c2, const b2Vec3& c3) : col1(c1), col2(c2), col3(c3)	{}

	///Set this matrix to all Zeros.
	void SetZero()
	{
		col1.SetZero();
		col2.SetZero();
		col3.SetZero();
	}

	///Solve A * x = b, Where b is a column vector. this is more efficient than computing the inverse in one-short case.
	b2Vec3 Solve33(const b2Vec3& b) const;

	///Solve A * x = b, Where b is a column vector,This is more efficient than computing the inverse in one-short case.
	///Solve only the upper 2-by-2 matrix equation.
	b2Vec2 Solve22(const b2Vec2& b) const;

	b2Vec3 col1, col2, col3;
};

/// A transform contains translation and rotation, It is used to represent the position and rotation of rigid frames.
struct b2XForm
{
	/// The default constructor does nothing (for performance).
	b2XForm()	{}

	/// Initialize using a position vector and a rotation matrix.
	b2XForm(const b2Vec2& position, const b2Mat22& R) : position(position), R(R)	{}

	/// Set this to the Identity transform.
	void SetIdentity()
	{
		position.SetZero();
		R.SetIdentity();
	}

	/// Set this base on the position and angle
	void Set(const b2Vec2& _position, float32 angle)
	{
		position  = _position;
		R.Set(angle);
	}

	/// Calculate the angle that the rotation matrix represent.
	float32 GetAngle() const
	{
		return b2Atan2(R.col1.y, R.col1.x);
	}

	b2Vec2 position;
	b2Mat22 R;
};

/// This describes the motion of a rigid/Shape for TOI computing.
/// Shapes are defined with the respect of the body origin, which may
/// no coincide with the center of mass, However, to support dynamics
/// We must interpolate the center of the mass.
struct b2Sweep
{
	/// Get the interpolated transform at a specified time.
	/// @param alpha is the factor in [0, 1], where 0 indicates t0.
	void GetTransform(b2XForm* xf, float32 alpha) const;

	/// Advance the sweep forward, yielding a new initial state.
	/// param t the new initial time.
	void Advance(float32 t);

	b2Vec2 localCenter;		/// Local center of the mass position.
	b2Vec2 c0, c;			/// center world position.
	float32 a0, a;			/// World angles.
	float32 t0;				/// Time interval = [t0, 1], where t0 is in [0, 1].
};

extern const b2Vec2 b2Vec2_zero;
extern const b2Mat22 b2Mat22_identity;
extern const b2XForm b2XForm_identity;

/// Perform the dot production on two vectors.
inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
{
	return a.x*b.x + a.y*b.y;
}

/// Perform the cross production on two vectors, in 2D this produces a scalar.
inline float32 b2Cross(const b2Vec2& a, const b2Vec2&  b)
{
	return a.x*b.y - a.y*b.x;
}

/// Perform the cross product on a vector and a scalar, In 2D this produces a vector.
inline b2Vec2 b2Cross(const b2Vec2& a, float32 scalar)
{
	return b2Vec2(scalar*a.y, -scalar*a.x);
}

/// Perform the cros  product on a scalar and a vector, In 2D this produces a vector.
inline b2Vec2 b2Cross(float32 scalar, const b2Vec2& a)
{
	return b2Vec2(-scalar*a.y, scalar*a.x );
}

/// Multiply a matrix transpose times a vector, if a rotation matrix was provided.
/// then this transform the vector from one frame to another.
inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
{
	return b2Vec2(A.col1.x*v.x + A.col2.x*v.y, A.col1.y*v.x + A.col2.y*v.y);
}

/// Multiply a matrix transpose times a vector, if a rotation matrix was provided.
/// then this transform the vector from one frame to another(inverse transform).
inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
{
	return b2Vec2(A.col1.x*v.x + A.col1.y*v.y, A.col2.x*v.x + A.col2.y*v.y);
}

/// Add two vectors component-wise.
inline b2Vec2 operator +(const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(a.x+b.x, a.y+b.y);
}

/// Subtract two vectors component-wise.
inline b2Vec2 operator -(const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(a.x-b.x, a.y-b.y);
}

inline b2Vec2 operator *(float32 s,const b2Vec2& a)
{
	return b2Vec2(s*a.x, s*a.y);
}

inline bool operator ==(const b2Vec2& a, const b2Vec2& b)
{
	return (a.x==b.x) && (a.y==b.y);
}

inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
{
	b2Vec2 c = a - b;
	return c.Length();
}

inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
{
	b2Vec2 c = a - b;
	return c.LengthSquared();
}

inline b2Vec3 operator *(float32 s, const b2Vec3& a)
{
	return b2Vec3(s*a.x, s*a.y, s*a.z);
}

/// Add two vectors component-wise.
inline b2Vec3 operator +(const b2Vec3& a, const b2Vec3& b)
{
	return b2Vec3(a.x+b.x, a.y+b.y, a.z+b.z);
}

/// Subtract two vectors component-wise.
inline b2Vec3 operator -(const b2Vec3& a, const b2Vec3& b)
{
	return b2Vec3(a.x-b.x, a.y-b.y, a.z-b.z);
}

/// Perform the dot product on two vectors.
inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
{
	return a.x*b.x + a.y*b.y + a.z*b.z;
}

/// Perform the cross product on two vectors.
inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
{
	return b2Vec3(a.y*b.z-a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y-a.y*b.x);
}

inline b2Mat22 operator +(const b2Mat22& A, const b2Mat22& B)
{
	return b2Mat22(A.col1+B.col1, A.col2+B.col2);
}

/// A*B
inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
{
	return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
}

/// A~T * B
inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
{
	b2Vec2 col1(b2Dot(A.col1, B.col1), b2Dot(A.col1, B.col2));
	b2Vec2 col2(b2Dot(A.col2, B.col1), b2Dot(A.col2, B.col2));
	return b2Mat22(col1, col2);
}

/// Multiplay a matrix times a vector.
inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
{
	return b2Vec3(A.col1.x*v.x+A.col2.x*v.y + A.col3.x*v.z, A.col1.y*v.x+A.col2.y*v.y + A.col3.y*v.z, A.col1.z*v.x+A.col2.z*v.y + A.col3.z*v.z);
}

inline b2Vec2 b2Mul(const b2XForm& T, const b2Vec2& a)
{
	return b2Mul(T.R, a) + T.position;
}

inline b2Vec2 b2MulT(const b2XForm& T, const b2Vec2& a)
{
	return b2MulT(T.R, a - T.position);	
}

inline b2Vec2 b2Abs(const b2Vec2& v)
{
	return b2Vec2(b2Abs(v.x), b2Abs(v.y));
}

inline b2Mat22 b2Abs(const b2Mat22& T)
{
	return b2Mat22(b2Abs(T.col1), b2Abs(T.col2));
}

template <typename T>
inline T b2Min(T a, T b)
{
	return a < b ? a : b;
}

inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
}

template <typename T>
inline T b2Max(T a, T b)
{
	return a > b ? a : b;
}

inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
{
	return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
}

template <typename T>
inline T b2Clamp(T a, T low, T high)
{
	return b2Max(low, b2Min(a, high));
}

inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
{
	return b2Max(low, b2Min(a, high));
}

template<typename T> inline void b2Swap(T& a, T& b)
{
	T tmp = a;
	a = b;
	b = tmp;
}

inline uint32 b2NextPowerOfTwo(uint32 x)
{
	x |= (x>>1);
	x |= (x>>2);
	x |= (x>>4);
	x |= (x>>8);
	x |= (x>>16);

	return x+1;
}

inline bool b2IsPowerOfTwo(uint32 x)
{
	bool result = (x>0) && (x&(x-1)) == 0;
	return result;
}

inline void b2Sweep::GetTransform(b2XForm* xf, float32 alpha) const
{
	b2Assert(NULL != xf);
	xf->position = (1.0f - alpha)*c0 + alpha*c;
	float32 angle = (1.0f - alpha)*a0 + alpha*a;
	xf->R.Set(angle);

	//shift to origin
	xf->position -= b2Mul(xf->R, localCenter);

}

inline void b2Sweep::Advance(float32 t)
{
	if(t0<t && (t-t0)>B2_FLT_EPSILON)
	{
		float32 alpha = (t-t0) / (1.0f - t0);
		c0 = (1.0f-alpha) * c0 + alpha*c;
		a0 = (1.0f-alpha) * a0 + alpha*a;
		t0 = t;
	}
}

#endif